/*
 *
 * Copyright(c) 2018-2019 chj <1019487871@qq.com>
 * QQ: 1019487871
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#include "los.h"

#define LOS_STATUS_STARTED      (1U)
#define LOS_PRIO_PLACEHOLDER   (struct los_thread_t *)(0X01)

static struct los_thread_control_t
{
    struct los_thread_t *thread_buf[LOS_THREAD_Y_BIT*LOS_THREAD_X_BIT];
    struct los_thread_t *running_thread;
    struct los_thread_t *ready_thread;
    struct los_thread_t *delay_thread;
    struct los_thread_t idle_thread;
    uint32_t ready[LOS_THREAD_Y_BIT];
    uint32_t asyn_signal_counter;
    uint64_t total_ms;
    uint8_t  status;
}los_thread;

int
los_fls_generic(uint32_t word)
{
    int bit = 32;

    if (!word) bit -= 1;
    if (!(word & 0xffff0000)) { word <<= 16; bit -= 16; }
    if (!(word & 0xff000000)) { word <<= 8; bit -= 8; }
    if (!(word & 0xf0000000)) { word <<= 4; bit -= 4; }
    if (!(word & 0xc0000000)) { word <<= 2; bit -= 2; }
    if (!(word & 0x80000000)) { word <<= 1; bit -= 1; }

    return bit;
}

int
los_ffs(uint32_t word)
{
    return los_fls_generic(word & (~word + 1)) - 1;
}

int
los_fls(uint32_t word)
{
    return los_fls_generic(word) - 1;
}

struct los_thread_t *
los_get_running_thread(void)
{
    return los_thread.running_thread;
}
 
static struct los_thread_t *
los_select_thread_to_run(void)
{
    uint8_t j = 0;
    uint8_t i = 0;
    for(i=0;i<LOS_THREAD_Y_BIT;i++)
    {
        if(los_thread.ready[i]==0)
            continue;
        j = los_ffs(los_thread.ready[i]);
        return los_thread.thread_buf[(i*LOS_THREAD_X_BIT)+j];
    }
    return NULL;
}

void
los_thread_ready(uint16_t prio)
{
    uint32_t x_bit = 0;
    uint32_t y_bit = 0;
    y_bit = prio>>5;
    x_bit = prio - (y_bit<<5);
    los_thread.ready[y_bit] |= 1<<x_bit;
    los_thread.ready_thread = los_select_thread_to_run();
} 
 
static void
los_thread_cut_ready(uint16_t prio)
{
    uint32_t x_bit = 0;
    uint32_t y_bit = 0;
    y_bit = prio>>5;
    x_bit = prio - (y_bit<<5);
    los_thread.ready[y_bit] &= ~(1<<x_bit);
    los_thread.ready_thread = los_select_thread_to_run();
} 
 
void *
los_resiger_thread(void *thread_routine,uint32_t *param,uint32_t stk_size,uint16_t prio)
{
    struct los_thread_t *thread = (struct los_thread_t*)los_malloc(sizeof(struct los_thread_t));;
    size_t *mem = NULL;
    atom_low_t low = 0;
    LOS_ASSERT(thread==NULL);
    LOS_ASSERT(prio>=(LOS_THREAD_X_BIT*LOS_THREAD_Y_BIT));
    if(thread==NULL || (prio>=(LOS_THREAD_X_BIT*LOS_THREAD_Y_BIT)))
        return NULL;
    if(los_thread.thread_buf[prio]!=NULL)
        return NULL;
    if(thread_routine==NULL)
        return NULL;
    los_memset(thread,0,sizeof(struct los_thread_t));
    stk_size = (stk_size + (LOS_STACK_ALIGN_SIZE - 1)) & ~(LOS_STACK_ALIGN_SIZE - 1);
    LOS_ASSERT((stk_size%LOS_STACK_ALIGN_SIZE)!=0);
    mem = los_memalign(LOS_STACK_ALIGN_SIZE,stk_size);
    LOS_ASSERT(mem==NULL);
    if((mem==NULL))
        return NULL;
    thread->stack = (uint8_t*)((size_t)mem + stk_size);
    thread->stack_size = stk_size;
    thread->prio = prio;
    losbsp_reset_thread(thread_routine,thread,param);
    low = losbsp_low_atom_start();
    los_thread.thread_buf[prio] = thread;
    losbsp_low_atom_end(low);
    los_thread_ready(prio);
    losbsp_low_atom_end(low);
    return (void*)thread;
}
 
void
los_start_thread(void *thread)
{
    if(los_thread.status == LOS_STATUS_STARTED)
    {
        los_int_save_asyn();
        los_thread_ready(((struct los_thread_t *)thread)->prio);
        los_int_restore_asyn();
    }
    else
    {
        los_thread_ready(((struct los_thread_t *)thread)->prio);
    }
}

void 
los_int_save_asyn(void)
{
    if(los_thread.status != LOS_STATUS_STARTED)
        return;
    losbsp_cut_asyn();
    if(los_thread.asyn_signal_counter!=CN_LIMIT_UINT32)
        los_thread.asyn_signal_counter++;
}

void
los_int_restore_asyn(void)
{
    struct los_thread_t *temp = NULL;
    if(los_thread.status != LOS_STATUS_STARTED)
        return;
    if(los_thread.asyn_signal_counter!=0)
       los_thread.asyn_signal_counter--;
    if(los_thread.asyn_signal_counter==0)
    {
        if(los_thread.ready_thread!=los_thread.running_thread)
        {
            temp = los_thread.running_thread;
            los_thread.running_thread = los_thread.ready_thread;
            losbsp_switch_context(los_thread.ready_thread,temp);
        }
    }
}

void
los_int_switch_thread(void)
{
    struct los_thread_t *temp = NULL;
    if(los_thread.ready_thread!=los_thread.running_thread)
    {
        temp = los_thread.running_thread;
        los_thread.running_thread = los_thread.ready_thread;
        losbsp_switch_context_int(los_thread.ready_thread,temp);
    }
}

uint32_t
los_get_wakeup_from(void)
{
    return los_thread.running_thread->wakeup_from;
}

uint64_t
los_get_time_ms(void)
{
    return los_thread.total_ms;
}

void 
los_resume_delay(struct los_thread_t *delay_thread)
{
    if(los_thread.delay_thread->delay_next == NULL)  
        los_thread.delay_thread = NULL;
    else
    {
        if(delay_thread == los_thread.delay_thread)
        {
            los_thread.delay_thread = los_thread.delay_thread->delay_next;
        }
        delay_thread->delay_next->delay_prev = delay_thread->delay_prev;
        delay_thread->delay_prev->delay_next = delay_thread->delay_next;
    }
    delay_thread->delay_next = NULL;
    delay_thread->delay_prev = NULL;
    delay_thread->delay_end_time = los_get_time_ms();
}

static void 
los_add_to_delay(uint32_t ms)
{
    struct los_thread_t *thread = NULL;

    los_thread.running_thread->delay_end_time = los_get_time_ms() + ms;
    if(los_thread.delay_thread == NULL)    
    {
        los_thread.running_thread->delay_next = los_thread.running_thread;
        los_thread.running_thread->delay_prev = los_thread.running_thread;
        los_thread.delay_thread = los_thread.running_thread;
    }
    else
    {
        thread = los_thread.delay_thread;
        do
        {
            if(thread->delay_end_time <= 
                                los_thread.running_thread->delay_end_time)
            {
                thread = thread->delay_next;
            }
            else
                break;
        }while(thread != los_thread.delay_thread);
        los_thread.running_thread->delay_next = thread;
        los_thread.running_thread->delay_prev = thread->delay_prev;
        thread->delay_prev->delay_next = los_thread.running_thread;
        thread->delay_prev = los_thread.running_thread;
        if(los_thread.delay_thread->delay_end_time > 
                                los_thread.running_thread->delay_end_time)
            los_thread.delay_thread = los_thread.running_thread;
    }
}

void 
los_add_running_to_block(struct los_thread_t **head,uint8_t qsort,
                                        uint32_t timeout,uint32_t status)
{
    struct los_thread_t *thread = NULL;
    los_thread_cut_ready(los_thread.running_thread->prio);
    los_thread.running_thread->delay_prev = NULL;
    los_thread.running_thread->delay_next = NULL;

    los_thread.running_thread->sync_head = head;
    if(*head == NULL)
    {
        los_thread.running_thread->sync_next = los_thread.running_thread;
        los_thread.running_thread->sync_prev = los_thread.running_thread;
        *head = los_thread.running_thread;
    }
    else
    {
        thread = *head;
        if(qsort == CN_BLOCK_PRIO)   
        {
            do
            {
                if(thread->prio <= los_thread.running_thread->prio)
                    thread = thread->sync_next;
                else
                    break;
            }while(thread != *head);
            los_thread.running_thread->thread_status |= CN_BLOCK_PRIO_SORT;
            los_thread.running_thread->sync_next = thread;
            los_thread.running_thread->sync_prev = thread->sync_prev;
            thread->sync_prev->sync_next = los_thread.running_thread;
            thread->sync_prev = los_thread.running_thread;
            if((*head)->prio > los_thread.running_thread->prio)
                *head = los_thread.running_thread;
        }
        else
        {
            los_thread.running_thread->thread_status &= ~CN_BLOCK_PRIO_SORT;
            los_thread.running_thread->sync_next = thread;
            los_thread.running_thread->sync_prev = thread->sync_prev;
            thread->sync_prev->sync_next = los_thread.running_thread;
            thread->sync_prev = los_thread.running_thread;
        }
    }
    if(timeout != CN_TIMEOUT_FOREVER)
    {
        los_thread.running_thread->thread_status |= status + CN_STS_SYNC_TIMEOUT;
        los_add_to_delay(timeout);
    }
    else
    {
        los_thread.running_thread->thread_status |= status;  
    }
}

bool_t
los_thread_is_null(uint16_t prio)
{
    return los_thread.thread_buf[prio] == NULL ? true : false;
}

void
los_set_thread_placeholder(uint16_t prio)
{
    if(los_thread.thread_buf[prio] != NULL)
        return;
    los_thread.thread_buf[prio] = LOS_PRIO_PLACEHOLDER;
}

void
los_change_prio_for_ipc(uint16_t prio)
{
    uint16_t temp_prio = 0;
    atom_low_t low = 0;
    if(los_thread.running_thread->prio == prio)
        return;
    temp_prio = los_thread.running_thread->prio;
    low = losbsp_low_atom_start();
    los_thread_cut_ready(temp_prio);
    los_thread.thread_buf[temp_prio] = LOS_PRIO_PLACEHOLDER;
    los_thread.running_thread->prio = prio;
    los_thread.thread_buf[prio] = los_thread.running_thread;
    los_thread_ready(prio);
    losbsp_low_atom_end(low);
}

uint32_t 
los_delay_ms(uint32_t ms)
{
    struct los_thread_t *delay_thread = NULL;
    uint64_t start_time = 0;
    uint64_t end_time = 0;
    if(ms==0)
        return 0;
    if(los_thread.status!=LOS_STATUS_STARTED)
        return 0;
    los_int_save_asyn();
    start_time = los_get_time_ms();
    los_thread.running_thread->delay_end_time = start_time + ms;
    los_thread_cut_ready(los_thread.running_thread->prio);
    los_thread.running_thread->thread_status = CN_STS_THREAD_DELAY;
    if(los_thread.delay_thread==NULL)
    {
        los_thread.running_thread->delay_next = los_thread.running_thread;
        los_thread.running_thread->delay_prev = los_thread.running_thread;
        los_thread.delay_thread = los_thread.running_thread;
    }
    else
    {    
        delay_thread = los_thread.delay_thread;
        do
        {
            if(delay_thread->delay_end_time <= 
                                los_thread.running_thread->delay_end_time)
            {
                delay_thread = delay_thread->delay_next;
            }
            else
                break;
        }while(delay_thread != los_thread.delay_thread);
        los_thread.running_thread->delay_next = delay_thread;
        los_thread.running_thread->delay_prev = delay_thread->delay_prev;
        delay_thread->delay_prev->delay_next = los_thread.running_thread;
        delay_thread->delay_prev = los_thread.running_thread;
        if(los_thread.running_thread->delay_end_time < 
                                    los_thread.delay_thread->delay_end_time)
            los_thread.delay_thread = los_thread.running_thread;
    }
    los_int_restore_asyn();
    end_time = los_get_time_ms();
    return (uint32_t)(end_time - start_time);
} 

void
los_tick_isr(uint32_t tick)
{
    struct los_thread_t *delay_thread = NULL;
    los_thread.total_ms += tick;
    if(los_thread.delay_thread!=NULL)
    {
        delay_thread = los_thread.delay_thread;
        while(1)
        {
            if(delay_thread->delay_end_time <= los_thread.total_ms)
            {
                if(delay_thread->sync_head != NULL)
                {
                    if(*(delay_thread->sync_head) == delay_thread)
                    {
                        if(delay_thread->delay_next == delay_thread)
                        {
                            *(delay_thread->sync_head) = NULL;

                            delay_thread->delay_next = NULL;
                            delay_thread->delay_prev = NULL;
                        }
                        else                   
                        {
                            *delay_thread->sync_head = delay_thread->delay_next;
                            delay_thread->delay_prev->delay_next
                                                = delay_thread->delay_next;
                            delay_thread->delay_next->delay_prev
                                                = delay_thread->delay_prev;
                        }
                    }
                    else           
                    {
                        delay_thread->delay_prev->delay_next
                                            = delay_thread->delay_next;
                        delay_thread->delay_next->delay_prev
                                            = delay_thread->delay_prev;
                    }
                    delay_thread->sync_head = NULL;   
                }
                delay_thread->wakeup_from = delay_thread->thread_status;
                delay_thread->thread_status = CN_STS_THREAD_READY;
                if(delay_thread->delay_next==delay_thread)
                {
                    los_thread.delay_thread = NULL;
                    los_thread_ready(delay_thread->prio);
                    break;
                }
                else
                {
                    los_thread.delay_thread = delay_thread->delay_next;
                    delay_thread->delay_next->delay_prev = delay_thread->delay_prev;
                    delay_thread->delay_prev->delay_next = delay_thread->delay_next;
                    los_thread_ready(delay_thread->prio);
                    delay_thread = los_thread.delay_thread;
                }
            }else
                break;
        }
    }
}

static void los_idle_thread(uint32_t *param)
{
    param = *(&param);
    while(1)
    {
        ;
    }
}

void
los_start(void)
{
    struct los_thread_t *thread = NULL;
    thread = los_select_thread_to_run();
    if(thread!=NULL)
    {
        los_thread.status = LOS_STATUS_STARTED;
        if(los_thread.ready_thread!=los_thread.running_thread)
        {
            los_thread.running_thread = los_thread.ready_thread;
            losbsp_start_thread(los_thread.ready_thread);
        }
    }
    else
        while(1);
}

void
los_thread_init(void)
{
    struct los_thread_t *idle_thread = NULL;
    uint32_t i = 0;
    uint8_t *temp = (uint8_t*)&los_thread;
    for(i=0;i<sizeof(struct los_thread_control_t);i++)
    {
        temp[i] = 0;
    }
    idle_thread = los_resiger_thread(los_idle_thread,NULL,LOS_IDLE_THREAD_STACK_SIZE,
                                              (LOS_THREAD_Y_BIT*LOS_THREAD_X_BIT-1));
    if(idle_thread!=NULL)
        los_start_thread(idle_thread);
    else
        while(1);
}


